/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef SONAR_H_
#define SONAR_H_

#include <vector>
#include "Object.h"
#include "RayCluster.h"
#include "../MaCI/interfaces/Ranging/RangingTypes.hpp"
#include "Shapes.h"

class CWorld;
class CSonarSet;

#define NUM_OF_CLUSTERS 2
#define NUM_OF_RAYS_PER_CLUSTER 3
#define ECHO_TRESHOLD_ANGLE DegToRad(70.0)

class CSonar : public CObject
{
  public:
    CSonar(CWorld *World, CObject *ParentObject, dBodyID parentBodyID, dReal *posOffset, dMatrix3 rotOffset, 
          float range, float beamAngle, float direction);
    
    void Activate();
    
    void Draw();
    
    void Update(float StepSize);
    
    MaCI::Ranging::TDistance GetReading();
    
    void OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2);
    
  private:
    CCylinder *cylinder;
    TMaterial material;
    std::vector<CRayCluster*> rays;
    
    dBodyID parentBody;
    dSpaceID raySpaceID;
    
    float range;
    float beamAngle; //angle of the beam cone
    float direction; //the direction that is put to the distance struct generated 
    dReal *posOffset;
    dMatrix3 rotOffset;
    
    float distance;
    
    //collision callback for rays
    static void RayCallback(void *data, dGeomID o1, dGeomID o2);
    
};

#endif /*SONAR_H_*/
